﻿#define ASTAR_USING_HEAP

using System;
using System.Collections;
using System.Collections.Generic;
using Gameplay.PVE;
using Gameplay.PVE.Data;
using Gameplay.PVE.Entity;
using Gameplay.PVE.Utils;
using Pathfinding;
using Unity.Profiling;
using UnityEditor;
using UnityEngine;
using UnityEngine.SocialPlatforms;
using Random = UnityEngine.Random;


namespace Gameplay.PVE
{
    public enum AStarBlockType
    {
        All = 0,
        Ground = 1,
        Air = 2,
        
    }
    public class AStarMgr
    {
        private static AStarMgr p_instance;

        public static AStarMgr Instance
        {
            get
            {
                if (p_instance == null)
                {
                    p_instance = new AStarMgr();
                }

                return p_instance;
            }
        }

        
        private int mapW;
        private int mapH;

        private AStarNode[,] nodes;

        private List<AStarNode> openList;
        private int openListCount;

        private HashSet<AStarNode> closeList;
        private int closeListCount;

        private PriorityQueue<AStarNode> openHeap;

        private Vector2Int startPos;
        private Vector2Int endPos;
        private float range;

        private AStarNodeInfo[,] grids;
        private GameObject root;


        private AStarNode[] aroundNodes;

        public static Vector2 gridSize = new Vector2(0.5f, 0.5f);

        private Vector3 offset;
        private int stageId = 0;

        private Quaternion direction;
        private Quaternion reverseDirection;
        
        public static bool ShowGrid = false;

        private SmoothModifier smoothModifier;
        
        public AStarMgr()
        {
            p_instance = this;
            smoothModifier = new SmoothModifier();
        }
        public enum EShapeType
        {
            round,
            rectange,
        }

        public class Obstacle
        {
            public int id;
            public Vector3 position;
            public Vector3 targetPosition;
            public EShapeType shape;
            public AStarBlockType blockType;
            public int width;
            public int height;
            public bool isStatic;
            public bool isMapBlock;
            public List<Vector2Int> blockList;
            public int stageId;
            public Vector3 offset;
        }

        private Dictionary<int, Obstacle> obstacles = new Dictionary<int, Obstacle>();

        public void InitMapInfo(int w, int h, Vector3 offset)
        {
            this.nodes = new AStarNode[w, h];
            for (int i = 0; i < w; i++)
            {
                for (int j = 0; j < h; j++)
                {
                    nodes[i, j] = new AStarNode(i, j, E_Node_Type.Walk);
                }
            }

            stageId = 0;
            if (root == null)
            {
                root = new GameObject("[AstarRoot]");
                root.transform.SetParent(PveScene.pveRootTrans);
            }
            mapW = w;
            mapH = h;
            this.offset = offset;
            aroundNodes = new AStarNode[8];
#if ASTAR_USING_HEAP
            openHeap = new PriorityQueue<AStarNode>(100,aStarNodeComparer);
#else
            openList = new List<AStarNode>(100);
            openListCount = 0;
#endif
            
            //closeList = new List<AStarNode>(100);
            closeListCount = 0;
            
            direction = Quaternion.identity;
            reverseDirection = Quaternion.identity;
            //DrawGrids();
        }

        public float DistanceToEdge(Vector3 position)
        {
            float xOffset = Mathf.Abs(position.x - offset.x);
            float zOffset = Mathf.Abs(position.z - offset.z);
            float x = mapW * gridSize.x / 2 - xOffset;
            float z = mapH * gridSize.y / 2 - zOffset;
            return x < z ? x : z;
        }

        public void UpdateForward(Vector3 forward)
        {
            direction = Quaternion.FromToRotation(Vector3.forward, forward);
            reverseDirection = Quaternion.FromToRotation(forward,Vector3.forward);
        }

        public void UpdateOffset(Vector3 offset)
        {
            offset = new Vector3(Mathf.RoundToInt(offset.x * 2) / 2f,0,Mathf.RoundToInt(offset.z * 2) / 2f);
            this.offset = offset;
            var count = 0;
            //吧上一关的阻挡点 相应的减去偏移
            foreach (var item in obstacles)
            {
                var obstacle = item.Value;
                if (obstacle.isStatic) //  && obstacle.offset != offset)
                {
                    for (int i = 0; i < obstacle.blockList.Count; i++)
                    {
                        int x = obstacle.blockList[i].x;
                        int y = obstacle.blockList[i].y;
                        if (x < 0 || y < 0 || x >= nodes.GetUpperBound(0) || y >= nodes.GetUpperBound(1))
                        {
                           
                        }
                        else
                        {
                            nodes[x,y].RemoveBlock(obstacle.id,true);
                        }

                        x = obstacle.blockList[i].x - Mathf.RoundToInt((this.offset.x - obstacle.offset.x) / gridSize.x);
                        y = obstacle.blockList[i].y - Mathf.RoundToInt((this.offset.z - obstacle.offset.z) / gridSize.y);
                        obstacle.blockList[i] = new Vector2Int(x,y);
                       
                        
                    }
                    obstacle.offset = offset;
                }
            }

            foreach (var item in obstacles)
            {
                var obstacle = item.Value;
                if (obstacle.isStatic)
                {
                    for (int i = 0; i < obstacle.blockList.Count; i++)
                    {
                        int x = obstacle.blockList[i].x;
                        int y = obstacle.blockList[i].y;
                        if (x < 0 || y < 0 || x >= nodes.GetUpperBound(0) || y >= nodes.GetUpperBound(1))
                        {
                            
                            count++;
                        }
                        else
                        {
                            nodes[x,y].AddBlock(obstacle.id,true);
                        }
                    }
                }
            }
        }

        public void ToNext(int stageId)
        {
            this.stageId = stageId;
            List<int> removeList = new List<int>();
            foreach (var item in obstacles)
            {
                if (item.Value.isStatic && item.Value.stageId != stageId)
                {
                    removeList.Add(item.Key);
                }
            }

            for (int i = 0; i < removeList.Count; i++)
            {
                RemoveObstacle(removeList[i]);
            }
        }

        public void GetRandomTargetPoint(ref List<Vector3> pathList,Vector3 start,int unitId)
        {
            List<AStarNode> list = new List<AStarNode>();
            for (int i = 0; i < mapW; i++)
            {
                for (int j = 0; j < mapH; j++)
                {
                    if (nodes[i,j].type == E_Node_Type.Walk)
                    {
                        list.Add(nodes[i,j]);
                    }
                }
            }

            var targetPos = list[Random.Range(0, list.Count)];
            var pos = new Vector3(targetPos.x * gridSize.x - mapW * gridSize.x / 2f, 0,
                targetPos.y * gridSize.y - mapH * gridSize.y / 2f) + offset;
            FindPath(ref pathList, unitId, start, pos);
        }

        public void AddBlockUnit(int id, Vector3 position, int width, int height, int blockType)
        {
            AStarBlockType eBlockType = (AStarBlockType)blockType;
            AddObstacle(id,position,width,height,eBlockType);
        }
        
        public void AddObstacle(int id, Vector3 position, int width, int height,AStarBlockType blockType = AStarBlockType.Ground)
        {
            if (obstacles.ContainsKey(id))
            {
                return;
            }
            obstacles.Add(id,new Obstacle
            {
                id = id,
                width = width,
                height = height,
                position = position,
                blockType = blockType,
            });
            var center = GetIndexByPosition(position - offset);
            nodes[center.x, center.y].AddBlock(id);
            SignAroundBlocks(id,center,width,height,true,blockType);
        }

        public void AddStaticBlockPoint(Vector3 point)
        {
            var center = GetIndexByPosition(point - offset);
            if (center.x > nodes.GetUpperBound(0))
            {
                Debug.LogErrorFormat("{0}的x越界,{1}>{2},边界宽度{3}",point.x,center.x,nodes.GetUpperBound(0),mapW);
            }
            else if (center.x < nodes.GetLowerBound(0))
            {
                Debug.LogErrorFormat("{0}的x越界,{1}<{2},边界宽度{3}",point.x,center.x,nodes.GetLowerBound(0),mapW);
            }
            else if (center.y < nodes.GetLowerBound(1))
            {
                Debug.LogErrorFormat("{0}的y越界,{1}<{2},边界高度{3}",point.y,center.y,nodes.GetLowerBound(1),mapH);
            }
            else if (center.y > nodes.GetUpperBound(1))
            {
                Debug.LogErrorFormat("{0}的y越界,{1}<{2},边界高度{3}",point.y,center.y,nodes.GetUpperBound(1),mapH);
            }
            nodes[center.x, center.y].AddBlock(9999,true);
        }
        
        public void AddStaticObstacle(int id, Vector3 position,RpgLevelData.RpgObstacle info,int stageId)
        {
            if (obstacles.ContainsKey(id))
            {
                return;
            }

            var obstacle = new Obstacle
            {
                id = id,
                width = info.width,
                height = info.height,
                position = position,
                isStatic = true,
                stageId = stageId,
                offset = offset,
            };
            obstacles.Add(id,obstacle);
            SignAroundBlocksStatic(obstacle,info);
        }
        
        //地图的阻挡点
        public void AddStaticBlocks(List<List<bool>> blocks,Vector3 position)
        {
            int id = 999999;
            var obstacle = new Obstacle
            {
                id = id,
                position = position,
                isStatic = true,
                stageId = stageId,
                offset = this.offset,
                blockList = new List<Vector2Int>(),
            };
            var offset = this.offset - new Vector3(mapW * gridSize.x / 2,0,mapH * gridSize.y / 2) - position;
            for (int i = 0; i < blocks.Count; i++)
            {
                for (int j = 0; j < blocks[i].Count; j++)
                {
                    if (blocks[i][j])
                    {

                        var offsetX = (int)Mathf.Round(offset.x / gridSize.x);
                        var offsetY = (int)Mathf.Round(offset.z / gridSize.y);
                        int x = i - offsetX;
                        int y = j - offsetY;
                        obstacle.blockList.Add(new Vector2Int(x,y));
                        if (x < 0 || y < 0 || x >= nodes.GetUpperBound(0) || y >= nodes.GetUpperBound(1))
                        {
                            
                        }
                        else
                        {
                            
                            nodes[x,y].AddBlock(id,true);
                        }
                    }
                }
            }
            obstacles.Add(id,obstacle);
        }
        
        public void AddObstacle(int id, Vector3 position, Vector3 size,AStarBlockType blockType = AStarBlockType.Ground)
        {
            int width = Mathf.CeilToInt(Math.Abs(size.x) / gridSize.x);
            int height = Mathf.CeilToInt(Math.Abs(size.z) / gridSize.y);
            AddObstacle(id,position,width,height,blockType);
        }

        public void RemoveObstacle(int id)
        {
            if (obstacles.TryGetValue(id, out var obstacle))
            {
                if (obstacle.isStatic)
                {
                    for (int i = 0; i < obstacle.blockList.Count; i++)
                    {
                        int x = obstacle.blockList[i].x;
                        int y = obstacle.blockList[i].y;
                        if (x < 0 || y < 0 || x >= nodes.GetUpperBound(0) || y >= nodes.GetUpperBound(1))
                        {
                            continue;
                        }
                        nodes[x,y].RemoveBlock(id,true);
                    }
                }
                obstacles.Remove(id);
            }
            
        }

        public bool CheckIsClear(Vector3 position, int width, int height)
        {
            float gridW = AStarMgr.gridSize.x;
            float gridH = AStarMgr.gridSize.y;
           
            var center = position;

            corners[0] = new Vector3(-width / 2f * gridW,0,-height / 2f * gridH);
            corners[1] = new Vector3(-width / 2f * gridW,0,height / 2f * gridH);
            corners[2] = new Vector3(width / 2f * gridW,0,height / 2f * gridH);
            corners[3] = new Vector3(width / 2f * gridW,0,-height / 2f * gridH);

            float parentY = 0;
            for (int i = 0; i < corners.Length; i++)
            {
                corners[i] = Quaternion.AngleAxis(parentY, Vector3.up) * corners[i];
            }
            
            int halfWidth = Mathf.CeilToInt(width / 2f);
            int halfHeight = Mathf.CeilToInt(height / 2f);
            int region = halfWidth + halfHeight;
            
            for (int i = 0; i < 4; i++)
            {
                cornersV2[i] = new Vector2(corners[i].x,corners[i].z);
            }
            for (float i = -region; i <= region; i ++)
            {
                for (float j = -region; j <= region; j ++)
                {
                    bool isBlock = true;
                    var blockPos = new Vector2(i * gridW,j * gridH);
                    for (int k = 0; k < 4; k++)
                    {
                        var cornerStart = cornersV2[k % 4];
                        var cornerEnd = cornersV2[(k + 1) % 4];
                        //判断blockPos是否在矩形范围内
                        if (PveUtils.GetIntersection(cornerStart, cornerEnd, Vector2.zero, blockPos) != Vector2.zero)
                        {
                            //不在范围内
                            isBlock = false;
                            break;
                        }
                    }

                    if (isBlock)
                    {
                        var centerGrid = GetIndexByPosition(center - offset);
                        int x = (int) (centerGrid.x + i);
                        int y = (int) (centerGrid.y + j);
                        if (x < 0 || y < 0 || x >= nodes.GetUpperBound(0) || y >= nodes.GetUpperBound(1))
                        {
                            continue;
                        }

                        if (!nodes[x, y].IsClear(0) || nodes[x, y].type == E_Node_Type.Stop)
                        {
                            return false;
                        }
                    }
                }
            }

            return true;
        }
        
        public void UpdateObstaclePosition(int id, Vector3 position,Vector3 targetPosition)
        {
            if (obstacles.TryGetValue(id, out var obstacle))
            {
                //var center = GetIndexByPosition(obstacle.position - offset,id);
                //nodes[center.x,center.y].ClearBlocks();
                obstacle.position = position;
                obstacle.targetPosition = targetPosition;
                /*center = GetIndexByPosition(obstacle.position - offset,id);
                 nodes[center.x, center.y].AddBlock(id);
                 SignAroundBlocks(id,center,obstacle.width,obstacle.height);*/
            }
        }

        public void ClearStage()
        {
            obstacles.Clear();
            for (int i = 0; i < mapW; i++)
            {
                for (int j = 0; j < mapH; j++)
                {
                    nodes[i,j].ClearStatic();
                }
            }
        }
        
        public void UpdateMapCollider()
        {
            updateColliderProfiler.Begin();
            for (int i = 0; i < mapW; i++)
            {
                for (int j = 0; j < mapH; j++)
                {
                    nodes[i,j].ClearBlocks();
                }
            }
            foreach (var item in obstacles.Values)
            {
                if (item.isStatic)
                {
                    continue;
                }
                var center = GetIndexByPosition(item.position - offset);
                nodes[center.x, center.y].AddBlock(item.id,false,item.blockType);
                SignAroundBlocks(item.id,center,item.width,item.height,true,item.blockType);
                
                /*center = GetIndexByPosition(item.targetPosition - offset,item.id);
                nodes[center.x, center.y].AddBlock(item.id);
                SignAroundBlocks(item.id,center,1,1);*/
            }
            /*for (int i = 0; i < mapW; i++)
            {
                for (int j = 0; j < mapH; j++)
                {
                    nodes[i, j].CheckNeedNotifyRepath();
                }
            }*/

            root.transform.position = offset;// - new Vector3(mapW / 2f * gridSize.x,0,mapH / 2f * gridSize.y);
            if (ShowGrid)
            {
                DrawGrids();
            }
            updateColliderProfiler.End();
        }
        
        public void DrawGrids()
        {
#if UNITY_EDITOR
            if (grids == null && mapW != 0)
            {
                grids = new AStarNodeInfo[mapW,mapH];
                var prefab = AssetDatabase.LoadAssetAtPath<GameObject>(
                    "Assets/ResourcesAssets/Pve/Prefabs/Others/astarNode.prefab");
                for (int i = 0; i < mapW; i++)
                {
                    for (int j = 0; j < mapH; j++)
                    {
                        var obj = GameObject.Instantiate(prefab);
                        obj.name = string.Format("[{0},{1}]", i, j);
                        obj.transform.SetParent(root.transform);
                        obj.transform.localPosition = new Vector3((i) * gridSize.x,0.2f,(j) * gridSize.y) - new Vector3(mapW / 2f,0,mapH / 2f) * gridSize.x;
                        obj.transform.localPosition = direction * obj.transform.localPosition;
                        obj.transform.forward = direction * Vector3.forward;
                        //obj.transform.SetActive(nodes[i,j].type == E_Node_Type.Stop);
                        //obj.transform.localScale = new Vector3(gridSize.x / 10 * 0.9f, 0, gridSize.y / 10 * 0.9f);
                        var info = obj.GetComponent<AStarNodeInfo>();
                        info.Init(i, j);
                        grids[i, j] = info;
                    }
                }
            }

            for (int i = 0; i < mapW; i++)
            {
                for (int j = 0; j < mapH; j++)
                {
                    bool isBlock = nodes[i, j].type == E_Node_Type.Stop || !nodes[i, j].IsClear(0,AStarBlockType.Ground);
                    grids[i,j].UpdateBlock(nodes[i, j].maxClearDistance,isBlock);
                    //grids[i, j].name = string.Format("[{0},{1}]{2}", i, j, nodes[i, j].maxClearDistance);
                }
            }
#endif
        }

        public void UpdateBlock(int x, int y,int id,bool isBlock)
        {
            nodes[x,y].ClearBlocks();
            if (isBlock)
            {
                nodes[x,y].AddBlock(id);
            }
        }

        private Vector2Int GetIndexByPosition(Vector3 position,bool autoSelectNear = false)
        {
            position = reverseDirection * position;
            int x = (int) (Mathf.Round(position.x / gridSize.x  + mapW / 2f));
            int y = (int) (Mathf.Round(position.z / gridSize.y + mapH / 2f));
            if (x < 0 || y < 0)
            {
                if (autoSelectNear)
                {
                    x = x < 0 ? 0 : x;
                    y = y < 0 ? 0 : y;
                }
                else
                {
                    return Vector2Int.zero;
                }
                
            }
            if (x >= mapW)
            {
                x = mapW - 1;
            }

            if (y >= mapH)
            {
                y = mapH - 1;
            }
            return new Vector2Int(x,y);
        }

        public bool IsSameGrid(Vector3 a, Vector3 b)
        {
            var indexA = GetIndexByPosition(a- offset);
            var indexB = GetIndexByPosition(b- offset);
            return indexA.x == indexB.x && indexA.y == indexB.y;
        }

        public bool IsCanWalkForward(Vector3 position, Vector3 forward, int id)
        {
            var targetPosition = position + forward.normalized * 0.5f;
            if (!IsWalkable(targetPosition, id))
            {
                return false;
            }
            targetPosition = position + forward.normalized * 1f;
            if (!IsWalkable(targetPosition, id))
            {
                return false;
            }
            return true;
        }

        public bool GetFirstAvailablePosition(ref Vector3 position, Vector3 forward, int id)
        {
            for (int i = 0; i < 6; i++)
            {
                float d = 2 + i * 0.5f;
                var pos = position + forward.normalized * d;
                if (IsWalkable(pos, id))
                {
                    position = pos;
                    return true;
                }
            }
            return false;
        }

        public bool CheckIsWalkable(Vector3 position, int id, int blockType = 0)
        {
            return IsWalkable(position, id, (AStarBlockType) blockType);
        }
        
        
        public bool IsWalkable(Vector3 position,int id,AStarBlockType blockType = AStarBlockType.All)
        {
            checkWalkableProfiler.Begin();
            var pos = GetIndexByPosition(position-offset);
            //开始位置的单位除去自己的碰撞
            
            var haveBlock = nodes[pos.x,pos.y].RemoveBlock(id);
            if (haveBlock)
            {
                SignAroundBlocks(id,pos,2,2,false);
            }
            

            range = 0;
            var isWalkable = IsValid(pos.x,pos.y,out var isClear,false,blockType);
            
            //开始位置的单位除去自己的碰撞再加回来
            if (id != -1 && haveBlock)
            {
                nodes[pos.x, pos.y].AddBlock(id);
                SignAroundBlocks(id, pos,2,2);
            }
            checkWalkableProfiler.End();
            return isWalkable;
        }

        public List<Vector3> GetNewList()
        {
            return new List<Vector3>();
        }

        public void ListCutTail(List<Vector3> list,int remainCount)
        {
            var count = list.Count - remainCount;
            for (int i = 0; i < count; i++)
            {
                list.RemoveAt(list.Count - 1);
            }
        }

        public bool IsOutOfBound(Vector3 position)
        {
            position -= offset;
            position = reverseDirection * position;
            int x = (int) (Mathf.Round(position.x / gridSize.x) + mapW / 2f);
            int y = (int) (Mathf.Round(position.z / gridSize.y) + mapH / 2f);
            return x < 0 || y < 0 || x >= mapW || y >= mapH;
        }

        public Vector3 GetClosestPointInEdge(Vector3 position)
        {
            position -= offset;
            position = reverseDirection * position;
            int x = (int) (Mathf.Round(position.x / gridSize.x)) + mapW / 2;
            int y = (int) (Mathf.Round(position.z / gridSize.y)) + mapH / 2;
            x = Mathf.Min(x, mapW);
            x = Mathf.Max(x, 0);
            y = Mathf.Min(y, mapH);
            y = Mathf.Max(y, 0);
            var pos = new Vector3(x * gridSize.x - mapW * gridSize.x / 2f, 0,
                y * gridSize.y - mapH * gridSize.y / 2f);
            pos = direction * pos + offset;
            return pos;
        }

        private bool FindAStarValidPointInRange(Vector2Int center, int range,out Vector2Int result)
        {
            var hasValid = false;
            result = center;
            for (int j = center.x - range; j <= center.x + range; j++)
            {
                if (j < 0 || j >= mapW)
                {
                    continue;
                }
                for (int k = center.y - range; k <= center.y + range; k++)
                {
                    if (k < 0 || k >= mapH)
                    {
                        continue;
                    }

                    if (k != center.y + range && k != center.y - range && j != center.x - range && j != center.x + range)
                    {
                        continue;
                    }
                    
                    var node = nodes[j, k];
                    if (node.IsClear(range))
                    {
                        hasValid = true;
                        result = new Vector2Int(node.x,node.y);
                        break;
                    }
                }
            }
            return hasValid;
        }
        
        public Vector3 GetAStarValidPoint(int id,Vector3 position)
        {
            Vector3 finalPosition = position;
            var end = position - offset;
            var center = GetIndexByPosition(end,true);
            var node = nodes[center.x,center.y];
            //无视自己
            var haveBlock = node.RemoveBlock(id);
            if (haveBlock)
            {
                SignAroundBlocks(id,center,2,2,false);
            }
            if (node.IsClear(range))
            {
                finalPosition = position;
            }
            else
            {
                for (int i = 1; i <= 3; i++)
                {
                    if (FindAStarValidPointInRange(center, i, out var result))
                    {
                        var pos = new Vector3(result.x * gridSize.x - mapW * gridSize.x / 2f, 0,
                            result.y * gridSize.y - mapH * gridSize.y / 2f);
                        pos = direction * pos + offset;
                        finalPosition = pos;
                        break;
                    }
                }
            }
            
            //开始位置的单位除去自己的碰撞再加回来
            if (id != -1 && haveBlock)
            {
                node.AddBlock(id);
                SignAroundBlocks(id, center,2,2);
            }
            return finalPosition;
        }
        
        private List<AStarNode> findPathNodelist = new List<AStarNode>(400);
        private List<AStarNode> targetAroundNodelist = new List<AStarNode>(400);

        private Dictionary<int, AStarNode> unitDestinations = new Dictionary<int, AStarNode>();
        private static ProfilerMarker findPathProfiler = new ProfilerMarker("AStarMgr.FindPath");
        private static ProfilerMarker updateColliderProfiler = new ProfilerMarker("AStarMgr.UpdateColliderProfiler");
        private static ProfilerMarker checkWalkableProfiler = new ProfilerMarker("AStarMgr.CheckWalkableProfiler");
        
        private static ProfilerMarker findPathProfiler1 = new ProfilerMarker("AStarMgr.FindPath.Stage1");
        private static ProfilerMarker findPathProfiler2 = new ProfilerMarker("AStarMgr.FindPath.Stage2");
        private static ProfilerMarker findPathProfiler3 = new ProfilerMarker("AStarMgr.FindPath.Stage3");
        private static ProfilerMarker findPathProfiler4 = new ProfilerMarker("AStarMgr.FindPath.Stage4");
        private static ProfilerMarker findPathProfiler5 = new ProfilerMarker("AStarMgr.FindPath.Stage5");
        private static ProfilerMarker findPathProfiler6 = new ProfilerMarker("AStarMgr.FindPath.Stage6");
        private static ProfilerMarker findPathProfiler7 = new ProfilerMarker("AStarMgr.FindPath.Stage7");
        private static ProfilerMarker findPathProfiler8 = new ProfilerMarker("AStarMgr.FindPath.Stage8");
        private static ProfilerMarker findPathProfiler9 = new ProfilerMarker("AStarMgr.FindPath.Stage9");
        
        private static ProfilerMarker findPathProfilerIsValid = new ProfilerMarker("AStarMgr.FindPath.IsValid");
        private static ProfilerMarker findPathSmoothModifier = new ProfilerMarker("AStarMgr.FindPath.Smooth");

        private class AStarNodeComparer : IComparer<AStarNode>
        {
            public int Compare(AStarNode a, AStarNode b)
            {
                if (a.f < b.f)
                    return 1;
                else if (a.f == b.f)
                    return 0;
                else
                    return -1;
            }
        }
        private AStarNodeComparer aStarNodeComparer = new AStarNodeComparer();
        
        private class AStarNodeComparerList : IComparer<AStarNode>
        {
            public int Compare(AStarNode a, AStarNode b)
            {
                if (a.f >= b.f)
                    return 1;
                else
                    return -1;
            }
        }
        private AStarNodeComparerList aStarNodeComparerList = new AStarNodeComparerList();

        public bool FindPathWithSmooth(ref List<Vector3> path, int id,Vector3 start,Vector3 end,float width = 1,bool isReverseFind = false)
        {
            var isSuccess = FindPath(ref path, id, start, end, width, isReverseFind);
            findPathSmoothModifier.Begin();
            smoothModifier.SmoothSimple(ref path);
            findPathSmoothModifier.End();
            return isSuccess;
        }

        private bool optimizeNoneBlockSegment = true;
        
        public bool FindPath(ref List<Vector3> path, int id,Vector3 start,Vector3 end,float width = 1,bool isReverseFind = false)
        {
            findPathProfiler.Begin();
            findPathProfiler1.Begin();
            /*if (unitDestinations.TryGetValue(id, out var destinationNode))
            {
                destinationNode.RemoveUnit(id);
                unitDestinations.Remove(id);
            }*/

            if (Vector3.Distance(start, end) >= 3)
            {
                isReverseFind = true;
            }
            path.Clear();
            range = 1;//width * width * 2;
            startPos = GetIndexByPosition(start-offset,true);
            endPos = GetIndexByPosition(end-offset,true);
#if ASTAR_USING_HEAP
            openHeap.Clear();
#else
            openList.Clear();
#endif
            closeListCount = 0;
            //closeList.Clear();
            for (int i = 0; i < mapW; i++)
            {
                for (int j = 0; j < mapH; j++)
                {
                    var node = nodes[i, j];
                    node.father = null;
                    node.f = 0;
                    node.g = 0;
                    node.h = 0;
                    node.isInOpenList = false;
                    node.isInCloseList = false;
                    node.isNoneBlock = false;
                }
            }
            findPathProfiler1.End();
            //开始位置的单位除去自己的碰撞
            bool haveBlock =  nodes[startPos.x,startPos.y].RemoveBlock(id);
            if (haveBlock)
            {
                SignAroundBlocks(id,startPos,2,2,false);
            }
            var list = findPathNodelist;
            list.Clear();
            //如果在同一个格子
            if (startPos.x == endPos.x && startPos.y == endPos.y)
            {
                var node = nodes[startPos.x, startPos.y];
                if (node.type == E_Node_Type.Walk)
                {
                    path.Add(start);
                    path.Add(end);
                    findPathProfiler.End();
                    return true;
                }
                else
                {
                    float range = this.range;
                    this.range = -1;
                    GetAroundNodes(startPos);
                    this.range = range;
                    //选择离终点最近的点
                    float distance = float.MaxValue;
                    AStarNode move = null;
                    Vector3 movePos = Vector3.zero;
                    for (int i = 0; i < aroundNodes.Length; i++)
                    {
                        var around = aroundNodes[i];
                        if (around != null)
                        {
                            var aroundPos = new Vector3(around.x * gridSize.x - mapW * gridSize.x / 2f, 0,
                                                around.y * gridSize.y - mapH * gridSize.y / 2f) + offset;
                            var d = Vector3.Distance(end, aroundPos);
                            if (d < distance)
                            {
                                distance = d;
                                move = around;
                                movePos = aroundPos;
                            }
                        }
                    }
                    if (move != null)
                    {
                        path.Add(start);
                        path.Add(movePos);
                        findPathProfiler.End();
                        return true;
                    }
                    else
                    {
                        path.Add(start);
                        findPathProfiler.End();
                        return true;
                    }
                }
            }
            var targetNode = nodes[endPos.x, endPos.y];
            var startNode = nodes[startPos.x, startPos.y];
            List<AStarNode> targetAroundNodeList = targetAroundNodelist;
            targetAroundNodeList.Clear();
            
            //如果起点本身是阻挡点，则在起点周围选择一个离终点最近的可行走的点作为起点
            if (!startNode.IsClear(range))
            {
                FindAroundValidNode(ref targetAroundNodeList, startPos);
                float distance = float.MaxValue;
                AStarNode node = null;
                for (int i = 0; i < targetAroundNodeList.Count; i++)
                {
                    var x = targetAroundNodeList[i].x - startPos.x;
                    var y = targetAroundNodeList[i].y - startPos.y;
                    var d = x * x + y * y;
                    if (d < distance)
                    {
                        distance = d;
                        node = targetAroundNodeList[i];
                    }
                }

                if (node != null)
                {
                    startPos.x = node.x;
                    startPos.y = node.y;
                }
                else
                {
                    Debug.Log(id + "起始点被包围");
                    path.Add(start);
                    path.Add(start);
                    findPathProfiler.End();
                    return false;
                }
            }
            targetAroundNodeList.Clear();
            //如果终点本身不可达，则选择终点最近的可达点作为终点
            if (!targetNode.IsClear(range))
            {
                FindAroundValidNode(ref targetAroundNodeList, endPos);
                float distance = float.MaxValue;
                AStarNode node = null;
                for (int i = 0; i < targetAroundNodeList.Count; i++)
                {
                    var x = targetAroundNodeList[i].x - startPos.x;
                    var y = targetAroundNodeList[i].y - startPos.y;
                    var d = x * x + y * y;
                    if (d < distance)
                    {
                        distance = d;
                        node = targetAroundNodeList[i];
                    }
                }

                if (node != null)
                {
                    endPos.x = node.x;
                    endPos.y = node.y;
                }
            }
            findPathProfiler5.Begin();
            findPathProfiler6.Begin();
            bool isFailed = false;
            
            if (isReverseFind)
            {
                var temp = startPos;
                startPos = endPos;
                endPos = temp;
                targetNode = nodes[endPos.x, endPos.y];
            }
            var currentNode = nodes[startPos.x, startPos.y];
            currentNode.h = Mathf.Abs(endPos.x - currentNode.x) + Mathf.Abs(endPos.y - currentNode.y);
            currentNode.g = 0;
            currentNode.f = (currentNode.h + currentNode.g) * 1.4f;
            currentNode.father = null;
            //closeList.Add(currentNode);
            currentNode.isInCloseList = true;
            closeListCount++;
            findPathProfiler6.End();
            findPathProfiler4.Begin();
#if ASTAR_USING_HEAP
            while (openHeap.Count + closeListCount < mapW * mapH)
#else
            while (openList.Count + closeListCount < mapW * mapH)
#endif
            {
                findPathProfiler2.Begin();
                var pos = new Vector2Int(currentNode.x,currentNode.y);
                bool isNoneBlock = GetAroundNodes(pos);
                findPathProfiler2.End();
                findPathProfiler3.Begin();

                //放入开启列表
                for (int i = 0; i < aroundNodes.Length; i++)
                {
                    var node = aroundNodes[i];
                    if (node != null)
                    {
                        //

                        if (currentNode.father != null)
                        {
                            node.g = currentNode.father.g + GetG(node,currentNode);
                        }
                        else
                        {
                            node.g = GetG(node,currentNode);
                        }
                        node.h = Mathf.Abs(endPos.x - node.x) + Mathf.Abs(endPos.y - node.y);
                        node.f = node.g + node.h;
                        node.father = currentNode;
                        node.isInOpenList = true;
                        node.isNoneBlock = isNoneBlock;
#if ASTAR_USING_HEAP
                        openHeap.Push(node);
#else
                        openList.Add(node);          
#endif
                    }
                }
                findPathProfiler3.End();
                //一次啥都没搜到，说明进入了死胡同
#if ASTAR_USING_HEAP
                if (openHeap.Count == 0)
#else
                if (openList.Count == 0)
#endif
                {
                    //closeList.Add(targetNode);
                    targetNode.isInCloseList = true;
                    closeListCount++;
                    targetNode.father = currentNode;
                    currentNode = targetNode;
                    isFailed = true;
                    break;
                }
                
                //找到开启列表中，消耗最小的点
                
#if ASTAR_USING_HEAP
                findPathProfiler8.Begin();
                var candidate = openHeap.Pop();
                candidate.isInOpenList = false;
                findPathProfiler8.End();
#else
                findPathProfiler7.Begin();
                openList.Sort(aStarNodeComparerList);
                findPathProfiler7.End();
                var candidate = openList[0];
                findPathProfiler8.Begin();
                openList.RemoveAt(0);
                findPathProfiler8.End();
#endif
                candidate.isInCloseList = true;
                closeListCount++;
                //closeList.Add(candidate);
                
                if (closeListCount > 300)//容错
                {
                    isFailed = true;
                    Debug.Log(id + "寻路失败");
                    break;
                }
                if (candidate.father == null || GetG(candidate, candidate.father) > GetG(candidate, currentNode))
                {
                    candidate.father = currentNode;
                }

                currentNode = candidate;
                
                if (candidate.x == endPos.x && candidate.y == endPos.y)//找到了终点
                {
                    break;
                }

                if (targetAroundNodeList.Count > 0 && targetAroundNodeList.Contains(candidate))//找到了周围的点
                {
                    break;
                }
            }
            findPathProfiler4.End();

            if (isFailed)
            {
                list.Add(startNode);
                list.Add(startNode);
            }
            else
            {
                findPathProfiler9.Begin();
                AStarNode endNode = currentNode;
                //unitDestinations.Add(id,endNode);
                //endNode.AddUnit(id);
                /*for (int i = 0; i < closeList.Count; i++)
                {
                    endNode = closeList[i];
                    if (endNode.father != null)
                    {
                        break;
                    }
                }*/
                int count = 0;
                while (endNode != null)
                {
                    if (!(!endNode.IsClear(range ) && endNode.x == endPos.x && endNode.y == endPos.y))
                    {
                        if (isReverseFind)
                        {
                            list.Add(endNode);
                        }
                        else
                        {
                            list.Insert(0,endNode);
                        }
                        
                    }

                    if (endNode == endNode.father)
                    {
                        break;
                    }
                    endNode = endNode.father;
                    count++;
                    if (count > 100)
                    {
                        Debug.LogError("寻路嵌套");
                        path.Add(start);
                        path.Add(start);
                        findPathProfiler.End();
                        return false;
                    }
                }
                findPathProfiler9.End();
            }
            //ProcessOutputNodes(ref list);
            
            if (isReverseFind)
            {
                var temp = startPos;
                startPos = endPos;
                endPos = temp;
            }
            findPathProfiler5.End();
            findPathProfiler6.Begin();
            //开始位置的单位除去自己的碰撞再加回来
            if (id != -1 && haveBlock)
            {
                nodes[startPos.x, startPos.y].AddBlock(id);
                SignAroundBlocks(id, startPos,2,2);
            }
            findPathProfiler6.End();
            findPathProfiler7.Begin();
            //判断是否有连续无阻挡的，这些都可以去掉
            bool isCanRemoveNode = true;
            for (int i = 0; i < list.Count; i++)
            {
                bool isNoneBlock;
                if (i == 0 || i == list.Count - 1)
                {
                    isNoneBlock = false;
                }
                else
                {
                    isNoneBlock = list[i].isNoneBlock;
                }
                if (isNoneBlock)
                {
                    if (!isCanRemoveNode)
                    {
                        isCanRemoveNode = true;
                    }
                    else
                    {
                        list.RemoveAt(i);
                        i--;
                    }
                }
            }
            
            for (int i = 0; i < list.Count; i++)
            {
                var pos = new Vector3(list[i].x * gridSize.x - mapW * gridSize.x / 2f, 0,
                              list[i].y * gridSize.y - mapH * gridSize.y / 2f);
                pos = direction * pos + offset;
                if (startPos.x == list[i].x && startPos.y == list[i].y)
                {
                    path.Add(start);
                    if (list.Count == 1)
                    {
                        path.Add(pos);
                    }
                }
                /*else if (i == list.Count - 1 && endPos.x == list[i].x && endPos.y == list[i].y)//判断最终点是不是和目标点在一个格子内
                {
                    path.Add(end);
                }*/
                else
                {
                    path.Add(pos);
                }
                //Debug.LogError(path[i]);
            }
#if ASTAR_USING_HEAP
            openHeap.Clear();
#else
            openList.Clear();
#endif
            //closeList.Clear();
            findPathProfiler7.End();
            findPathProfiler.End();
            return !isFailed;
        }

        //把两个点之间没有阻挡的点删除，尽可能减少节点  
        private void ProcessOutputNodes(ref List<AStarNode> list)
        {
            //一下子处理过多，会有性能问题
            int startIndex = 0;
            int endIndex = 2;
            while (endIndex < list.Count && startIndex < list.Count)
            {
                if (!CheckHasObstacle(list[startIndex],list[endIndex]))
                {
                    list.RemoveAt(startIndex + 1);
                }
                else
                {
                    
                    startIndex++;
                    endIndex = startIndex + 2;
                }
            }
        }

        
        Vector2[] squarePoints = new  Vector2[]{Vector2.zero,Vector2.zero,Vector2.zero,Vector2.zero};
        //判断两个点的连线经过多少个方块，这些方块中是否有阻挡   todo性能优化点
        private bool CheckHasObstacle(AStarNode start, AStarNode end)
        {
            int startX, endX, startY, endY;
            if (start.x > end.x)
            {
                startX = end.x;
                endX = start.x;
            }
            else
            {
                startX = start.x;
                endX = end.x;
            }

            if (start.y > end.y)
            {
                startY = end.y;
                endY = start.y;
            }
            else
            {
                startY = start.y;
                endY = end.y;
            }

            Vector2 startV = new Vector2(startX,startY);
            Vector2 endV = new Vector2(endX,endY);
            Vector3 dir = new Vector3(end.x - start.x,0,end.y - start.y);
            for (int i = startX; i <= endX; i++)
            {
                for (int j = startY; j <= endY; j++)
                {
                    bool isCross = false;
                    //先根据方块的四个顶点是否在连线的两边，判断连线是否穿过方块
                    squarePoints[0].x = i - 0.5f;
                    squarePoints[0].y = j - 0.5f;
                    squarePoints[1].x = i + 0.5f;
                    squarePoints[1].y = j - 0.5f;
                    squarePoints[2].x = i - 0.5f;
                    squarePoints[2].y = j + 0.5f;
                    squarePoints[3].x = i + 0.5f;
                    squarePoints[3].y = j + 0.5f;
                    float angle = 0;
                    for (int k = 1; k < 4; k++)
                    {
                        Vector2 begin = squarePoints[0];
                        Vector2 dist = squarePoints[k];
                        if (PveUtils.GetIntersection(startV, endV, begin, dist) != Vector2.zero)
                        {
                            isCross = true;
                            break;
                        }
                    }
                    //判断是否是阻挡
                    if (isCross)
                    {
                        if (!nodes[i, j].IsClear(range))
                        {
                            return true;
                        }
                    }
                }
            }
            return false;
        }
        
        //向外找两圈，找到可行的点
        private bool FindAroundValidNode(ref List<AStarNode> list,Vector2Int center)
        {
            bool isAroundValid = false;
            for (int j = center.x - 2; j <= center.x + 2; j++)
            {
                if (j < 0 || j >= mapW)
                {
                    continue;
                }
                for (int k = center.y - 2; k <= center.y + 2; k++)
                {
                    if (k < 0 || k >= mapH)
                    {
                        continue;
                    }

                    var node = nodes[j, k];
                    if (node.IsClear(range))
                    {
                        list.Add(node);
                        isAroundValid = true;
                    }
                }
            }
            return isAroundValid;
        }

        
        //从中心开始螺旋式往四周搜索，根据序号获取实际的位置
        private int GetIndexFromCenter(int sort)
        {
            if (sort % 2 == 0)
            {
                return -sort / 2;
            }
            return (sort + 1) / 2;
        }
        
        /// <summary>
        /// 根据传入的位置，返回合法的位置
        /// </summary>
        /// <param name="position"></param>
        /// <param name="width"></param>
        /// <param name="height"></param>
        /// <param name="range"></param>
        /// <returns></returns>
        public bool FindValidPosition(ref Vector3 position,int width, int height,int range = 5,PveTargetSearchHelper.RegionAdapter regionAdapter = null)
        {
            var center = GetIndexByPosition(position - offset);
            int centerX = center.x;
            int centerY = center.y;
            
            int realRange = range * 2;
            int totalCount = realRange * realRange;
            
            //从中心开始，往外扩圈

            for (int rangeIndex = 0; rangeIndex <= range; rangeIndex++)
            {
                //每轮只搜索边缘的
                for (int i = 0; i <= rangeIndex * 2; i++)
                {
                    //判断是否是边缘
                    for (int j = 0; j <= rangeIndex * 2; j++)
                    {
                        int x = GetIndexFromCenter(i);
                        int y = GetIndexFromCenter(j);
                        if (x == -rangeIndex || x == rangeIndex || y == rangeIndex || y == -rangeIndex)
                        {
                            x += centerX;
                            y += centerY;
                            if (x < 0 || x >= mapW || y < 0 || y >= mapH)
                            {
                                continue;
                            }
                            
                            var node = nodes[x, y];
                            var newPosition = new Vector3(node.x * gridSize.x - mapW * gridSize.x / 2f, 0,
                                                  node.y * gridSize.y - mapH * gridSize.y / 2f) + offset;
                            if (regionAdapter != null)
                            {
                                newPosition = regionAdapter(newPosition);
                            }
                            if (CheckIsClear(newPosition, width, height))
                            {
                                position = newPosition;
                                return true;
                            }
                        }
                    }
                }
            }
            
            return false;
        }
        
        private int SortOpenList(AStarNode a, AStarNode b)
        {
            if (a.f > b.f)
                return 1;
            else if (a.f == b.f)
                return 1;
            else
                return -1;
        }
        
        //对他周围的点，标记(动态）
        private void SignAroundBlocks(int id,Vector2Int center,int width,int height,bool add = true,AStarBlockType blockType = AStarBlockType.Ground)
        {
            if (width == 1)
            {
                width = 0;
            }
            if (height == 1)
            {
                height = 0;
            }
            int halfWidth = Mathf.CeilToInt(width / 2f);
            int halfHeight = Mathf.CeilToInt(height / 2f);
            for (int j = center.x - halfWidth; j <= center.x + halfWidth; j++)
            {
                if (j < 0 || j >= mapW)
                {
                    continue;
                }
                for (int k = center.y - halfHeight; k <= center.y + halfHeight; k++)
                {
                    if (k < 0 || k >= mapH)
                    {
                        continue;
                    }

                    if (add)
                    {
                        float x = center.x - j;
                        float y = center.y - k;
                        var distance = x * x + y * y;
                        nodes[j,k].UpdateClearArea(id,0,blockType);
                    }
                    else
                    {
                        nodes[j,k].RemoveAroundBlock(id);
                    }

                }
            }
        }
        Vector3[] corners = new Vector3[4];
        Vector2[] cornersV2 = new Vector2[4];
        //静态阻挡标记
        private void SignAroundBlocksStatic(Obstacle obstacle,RpgLevelData.RpgObstacle info)
        {
            float gridW = AStarMgr.gridSize.x;
            float gridH = AStarMgr.gridSize.y;
            int width = obstacle.width;
            int height = obstacle.height;
            var center = obstacle.position;
            int id = obstacle.id;
            var blockList = new List<Vector2Int>();
            obstacle.blockList = blockList;
            
            corners[0] = new Vector3(-width / 2f * gridW,0,-height / 2f * gridH);
            corners[1] = new Vector3(-width / 2f * gridW,0,height / 2f * gridH);
            corners[2] = new Vector3(width / 2f * gridW,0,height / 2f * gridH);
            corners[3] = new Vector3(width / 2f * gridW,0,-height / 2f * gridH);

            float parentY = info.rotation.y;
            for (int i = 0; i < corners.Length; i++)
            {
                corners[i] = Quaternion.AngleAxis(parentY, Vector3.up) * corners[i];
            }
            
            int halfWidth = Mathf.CeilToInt(width / 2f);
            int halfHeight = Mathf.CeilToInt(height / 2f);
            int region = halfWidth + halfHeight;
            
            for (int i = 0; i < 4; i++)
            {
                cornersV2[i] = new Vector2(corners[i].x,corners[i].z);
            }

            var offset = this.offset - new Vector3(mapW * gridSize.x / 2, 0, mapH * gridSize.y / 2) - center;
            for (int i = -region; i <= region; i ++)
            {
                for (int j = -region; j <= region; j ++)
                {
                    bool isBlock = true;
                    var blockPos = new Vector2(i * gridW,j * gridH);
                    for (int k = 0; k < 4; k++)
                    {
                        var cornerStart = cornersV2[k % 4];
                        var cornerEnd = cornersV2[(k + 1) % 4];
                        if (PveUtils.GetIntersection(cornerStart, cornerEnd, Vector2.zero, blockPos) != Vector2.zero)
                        {
                            isBlock = false;
                            break;
                        }
                    }

                    if (isBlock)
                    {
                        var offsetX = (int)Mathf.Round(offset.x / gridSize.x);
                        var offsetY = (int)Mathf.Round(offset.z / gridSize.y);
                        int x = i - offsetX;
                        int y = j - offsetY;
                        if (x < 0 || y < 0 || x >= nodes.GetUpperBound(0) || y >= nodes.GetUpperBound(1))
                        {
                            
                        }
                        else
                        {
                            nodes[x,y].AddBlock(id,true);
                        }
                        blockList.Add(new Vector2Int(x,y));
                    }
                }
            }
            /*//隐藏点也要作为阻挡点
            for (int i = 0; i < info.playerHidePoints.Count; i++)
            {
                var centerGrid = GetIndexByPosition(info.playerHidePoints[i] + PveManager.Instance.startPosition - offset);
                for (int j = -1; j <= 1; j++)
                {
                    for (int k = -1; k <= 1; k++)
                    {
                        int x = centerGrid.x + j;
                        int y = centerGrid.y + k;
                        nodes[x,y].AddBlock(id,true);
                        blockList.Add(new Vector2Int(x,y));
                    }
                }
               
            }
            for (int i = 0; i < info.enemyHidePoints.Count; i++)
            {
                var centerGrid = GetIndexByPosition(info.enemyHidePoints[i] + PveManager.Instance.startPosition - offset);
                for (int j = -1; j <= 1; j++)
                {
                    for (int k = -1; k <= 1; k++)
                    {
                        int x = centerGrid.x + j;
                        int y = centerGrid.y + k;
                        nodes[x,y].AddBlock(id,true);
                        blockList.Add(new Vector2Int(x,y));
                    }
                }
            }*/
        }
        
        private float GetG(AStarNode a,AStarNode b)
        {
            if (a.x == b.x)
            {
                return Mathf.Abs(a.y - b.y);
            }
            else if (a.y == b.y)
            {
                return Mathf.Abs(a.x - b.x);
            }
            else
            {
                return 1.4f;
            }
        }

        private bool GetAroundNodes(Vector2Int pos)
        {
            bool isClear = false;
            bool isNoneBlock = true;
            int index = 0;
            var posX = pos.x;
            var posY = pos.y;
            //左上
            var x = posX - 1;
            var y = posY+ 1;
            aroundNodes[index] = IsValid(x,y,out isClear) ? nodes[x,y] : null;
            isNoneBlock = isNoneBlock && isClear;
            index++;
            //上
            x = posX;
            y = posY + 1;
            aroundNodes[index] = IsValid(x,y,out isClear) ? nodes[x,y] : null;
            isNoneBlock = isNoneBlock && isClear;
            index++;
            //右上
            x = posX + 1;
            y = posY + 1;
            aroundNodes[index] = IsValid(x,y,out isClear) ? nodes[x,y] : null;
            isNoneBlock = isNoneBlock && isClear;
            index++;
            //右
            x = posX + 1;
            y = posY;
            aroundNodes[index] = IsValid(x,y,out isClear) ? nodes[x,y] : null;
            isNoneBlock = isNoneBlock && isClear;
            index++;
            //右下
            x = posX + 1;
            y = posY - 1;
            aroundNodes[index] = IsValid(x,y,out isClear) ? nodes[x,y] : null;
            isNoneBlock = isNoneBlock && isClear;
            index++;
            //下
            x = posX;
            y = pos.y - 1;
            aroundNodes[index] = IsValid(x,y,out isClear) ? nodes[x,y] : null;
            isNoneBlock = isNoneBlock && isClear;
            index++;
            //左下
            x = posX - 1;
            y = posY - 1;
            aroundNodes[index] = IsValid(x,y,out isClear) ? nodes[x,y] : null;
            isNoneBlock = isNoneBlock && isClear;
            index++;
            //左
            x = posX - 1;
            y = posY;
            aroundNodes[index] = IsValid(x,y,out isClear) ? nodes[x,y] : null;
            isNoneBlock = isNoneBlock && isClear;
            return isNoneBlock;
        }

        private bool IsValid(int x,int y,out bool isClear,bool considerStartEnd = true,AStarBlockType blockType = AStarBlockType.All)
        {
            isClear = true;
            findPathProfilerIsValid.Begin();
            if (x < 0 || x >= mapW || y < 0 || y >= mapH)
            {
                findPathProfilerIsValid.End();
                return false;
            }

            var node = nodes[x, y];
            bool isDifferentFromStartAndEnd = (node.x != endPos.x || node.y != endPos.y) &&
                                              (node.x != startPos.x || node.y != startPos.y);
            
            if (blockType == AStarBlockType.All)
            {
                isClear = node.IsClear(range);
            }
            else
            {
                isClear = node.IsClear(range, blockType);
            }
            if (!isClear && (!considerStartEnd || isDifferentFromStartAndEnd))
            {
                findPathProfilerIsValid.End();
                return false;
            }
#if ASTAR_USING_HEAP
            if (considerStartEnd && (node.isInCloseList || node.isInOpenList))
#else
            if (node.isInCloseList || node.isInOpenList)
#endif
            {
                findPathProfilerIsValid.End();
                return false;
            }

            findPathProfilerIsValid.End();
            return true;
        }

        public uint ShowPath(List<Vector3> pathList, DrawLineTool tool)
        {
            var points = pathList;
            List<Vector2> pointList = new List<Vector2>();
            for (int i = 0; i < points.Count; i++)
            {
                var point = points[i];
                pointList.Add(new Vector2
                {
                    x = point.x,
                    y = point.z
                });
            }

            LineInfo line = new LineInfo()
            {
                color = Color.white,
                speed = 1,
                isPart = true,
                isHeadPart = true,
            };
            return tool.AddMultiLines(line, pointList);
        }

        public void HidePath(uint id, DrawLineTool tool)
        {
            tool.DelMultiLines(id);
        }
        
        public void Dispose()
        {
            GameObject.DestroyImmediate(root);
            root = null;
#if ASTAR_USING_HEAP
            openHeap.Clear();
#else
            openList.Clear();
#endif
            closeList.Clear();
            obstacles.Clear();
            nodes = null;
            grids = null;
            aroundNodes = null;
        }
    }
}

